www.gusucode.com > Smart Nanosatellite Attitude Propagator (SNAP) 程序工具箱matlab源码 > Smart Nanosatellite Attitude Propagator (SNAP)/libastro/ecef2lla2.m
%% Copyright (c) 2006, Michael Kleder % All rights reserved. % By Michael Kleder (6/28/2005) (Updated 4/27/2006) % Edited by: Samir Rawashdeh (8/17/2011) % % Redistribution and use in source and binary forms, with or without % modification, are permitted provided that the following conditions are % met: % % * Redistributions of source code must retain the above copyright % notice, this list of conditions and the following disclaimer. % * Redistributions in binary form must reproduce the above copyright % notice, this list of conditions and the following disclaimer in % the documentation and/or other materials provided with the distribution % % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" % AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE % IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE % ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS % INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN % CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) % ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE % POSSIBILITY OF SUCH DAMAGE. % ECEF2LLA - convert earth-centered earth-fixed (ECEF) % cartesian coordinates to latitude, longitude, % and altitude % % USAGE: % [lat,lon,alt] = ecef2lla(x,y,z) % % lat = geodetic latitude (radians) % lon = longitude (radians) % alt = height above WGS84 ellipsoid (m) % x = ECEF X-coordinate (m) % y = ECEF Y-coordinate (m) % z = ECEF Z-coordinate (m) % % Notes: (1) This function assumes the WGS84 model. % (2) Latitude is customary geodetic (not geocentric). % (3) Inputs may be scalars, vectors, or matrices of the same % size and shape. Outputs will have that same size and shape. % (4) Tested but no warranty; use at your own risk. % (5) Michael Kleder, April 2006 function lla = ecef2lla2(p_ecef) % WGS84 ellipsoid constants: a = 6378137; e = 8.1819190842622e-2; for i = 1:size(p_ecef,2) x = p_ecef(1,i); y = p_ecef(2,i); z = p_ecef(3,i); % calculations: b = sqrt(a^2*(1-e^2)); ep = sqrt((a^2-b^2)/b^2); p = sqrt(x.^2+y.^2); th = atan2(a*z,b*p); lon = atan2(y,x); lat = atan2((z+ep^2.*b.*sin(th).^3),(p-e^2.*a.*cos(th).^3)); N = a./sqrt(1-e^2.*sin(lat).^2); alt = p./cos(lat)-N; % return lon in range [0,2*pi) lon = mod(lon,2*pi); % correct for numerical instability in altitude near exact poles: % (after this correction, error is about 2 millimeters, which is about % the same as the numerical precision of the overall function) k=abs(x)<1 & abs(y)<1; alt(k) = abs(z(k))-b; if lon > pi lon = lon - 2*pi; end lla(i,:) = [lat*180/pi, lon*180/pi, alt]; end return